/*
 * path.cpp
 *
 *  Created on: 08.03.2012
 *      Author: ignacio
 */

#include "path.h"
#include <cmath>

namespace neurignacio
{

real Segment::getParameterClosestToPoint(const Vector3& C) const
// Given segment ab and point c, computes closest point d on ab.
// Also returns t for the position of d, d(t) = a + t*(b - a)
{
	Vector3 AB(B-A);
	real t = (C-A) * AB / (AB*AB);
	if (t<0) t=0;
	if (t>1) t=1;

	return t;
}

Vector3 Segment::getPointFromParameter(const real& parameter) const
{
	return Vector3(A+(B-A)*parameter);
}

Vector3 Segment::closestToPoint(const Vector3& C) const
// Given segment ab and point c, computes closest point d on ab.
// Also returns t for the position of d, d(t) = a + t*(b - a)
{
	real t = getParameterClosestToPoint(C);

	return getPointFromParameter(t);
}

ostream& operator<<(ostream& out, const Segment& segment)
{
	out << segment.A << "->" << segment.B;
	return out;
}

template<>
real Path<Vector3>::getParameterClosestToPoint(const Vector3& C) const
{
//	cout << "Point " << C << endl;
	real t=0;
	unsigned int numOfSegment=0;
	real parameter=0;	// Stores the maximum paramenter found so far
	real distanceMin=REAL_MAX;
	for (Path::const_iterator point=this->begin(); point!=this->end()-1; ++point, ++numOfSegment)
	{
		Segment AB(*point, *(point+1));
		t = AB.getParameterClosestToPoint(C);
//		cout << "NumOfSegment=" << numOfSegment << "Segment: " << AB << " t=" << t;
		Vector3 closestPointInSegment = AB.getPointFromParameter(t);
//		cout << " closestPointInSegment: " << closestPointInSegment;
		Vector3 r = closestPointInSegment - C;
		real distance = r.length();
//		cout << " r=" << r << " distance=" << distance << " distanceMin=" << distanceMin << endl;
		if (distance < distanceMin)
		{
			parameter = t+numOfSegment;
			distanceMin=distance;
		}
	}
//	cout << "Output parameter: " << parameter;
	return parameter;	// Point C is past the end of the path
}

template<>
Vector3 Path<Vector3>::getPointFromParameter(const real& parameter) const
// parameter = segment (int) + t (real);
{
	if (parameter<0)	// First point of the path
		return (*this)[0];
	else if (parameter>=this->size()-1)	// Last point of the path
		return this->back();
	else
	{
		real segment;
		real t = std::modf(parameter, &segment);
//		cout << "parameter: " << parameter << " segment: " << segment << " t: " << t << endl;
		Segment AB((*this)[segment], (*this)[segment+1]);
		return AB.getPointFromParameter(t);
	}
}

template<>
Vector3 Path<Vector3>::closestToPoint(const Vector3& C) const
{
	real t=0;
	for (Path::const_iterator point=this->begin(); point!=this->end()-1; ++point)
	{
		Segment AB(*point, *(point+1));
		t = AB.getParameterClosestToPoint(C);

//		cout << "Segment: " << AB << " t=" << t << endl;
		if (t==0)
		{
			return *point;	// Point C is before the begining of the path
		}
		else if (t<1)
		{
			return AB.getPointFromParameter(t);	// Point C is between the actual segment
		}
	}
	return this->back();	// Point C is past the end of the path
}


} // end namespace neurignacio


